#include "sbus.h"
#include "string.h"

static int count = 0;
int rx_flag=0;
static uint8_t sbus_rx_buffer[32]={0};
static uint8_t rx_len=0;
int outOfControlFlag=0;
int sbus_channel[16]={1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000};
static int rx_step=0;
int rxFrameCount=0;
int rxErrorCount=0;
static void parser(void);

void uart_recv_cb(uint8_t ch)
{
	count = 0;
	switch(rx_step)
	{
		case 0:					//recv head
			if(ch==0x0F) 
			{
				rx_len = 0;
				sbus_rx_buffer[rx_len] = ch;
				rx_len ++;
				rx_step=1;
			}
			break;
		case 1:					//recv data
			sbus_rx_buffer[rx_len] = ch;
			rx_len ++;
			if(rx_len==24)
			{
				rx_step=2;
			}
			if(rx_len>24)
			{
				rx_step = 3;
			}
			break;
		case 2:					//recv success
			if(ch==0x00)
			{
				sbus_rx_buffer[rx_len] = ch;
				rx_len ++;
				parser();
				rx_flag = 1;
				outOfControlFlag = 0;
				rx_step = 0;
				rxFrameCount++;
			}
			else
			{
				rx_step = 3;
			}
			break;
		case 3:					//recv error
			rx_step = 0;
			rx_len = 0;
			rxErrorCount++;
			break;
	}	
}

static void parser(void)
{
	sbus_channel[ 0] = (sbus_rx_buffer[ 1] | (sbus_rx_buffer[2] << 8)) &0x07ff;                                     //!< Channel 0
	sbus_channel[ 1] = ((sbus_rx_buffer[ 2] >> 3) | (sbus_rx_buffer[3] << 5)) &0x07ff;                              //!< Channel 1
	sbus_channel[ 2] = ((sbus_rx_buffer[ 3] >> 6) | (sbus_rx_buffer[4] << 2) | (sbus_rx_buffer[5] << 10)) &0x07ff;  //!< Channel 2 			油门通道
	sbus_channel[ 3] = ((sbus_rx_buffer[ 5] >> 1) | (sbus_rx_buffer[6] << 7)) &0x07ff;    							//!< Channel 3
	sbus_channel[ 4] = ((sbus_rx_buffer[ 6] >> 4) | (sbus_rx_buffer[7] << 4)) &0x07ff;    							//!< Channel 4
	sbus_channel[ 5] = ((sbus_rx_buffer[ 7] >> 7) | (sbus_rx_buffer[8] << 1) | (sbus_rx_buffer[9] << 9)) &0x07ff;	//!< Channel 5
	sbus_channel[ 6] = ((sbus_rx_buffer[ 9] >> 2) | (sbus_rx_buffer[10] << 6)) &0x07ff;								//!< Channel 6
	sbus_channel[ 7] = ((sbus_rx_buffer[10] >> 5) | (sbus_rx_buffer[11] << 3)) &0x07ff;								//!< Channel 7
	sbus_channel[ 8] = (sbus_rx_buffer[12] | (sbus_rx_buffer[13] << 8)) &0x07ff;									//!< Channel 8
	sbus_channel[ 9] = ((sbus_rx_buffer[13] >> 3) | (sbus_rx_buffer[14] << 5)) &0x07ff;								//!< Channel 9
	sbus_channel[10] = ((sbus_rx_buffer[14] >> 6) | (sbus_rx_buffer[15] << 2) | (sbus_rx_buffer[16] << 10)) &0x07ff;//!< Channel 10
	sbus_channel[11] = ((sbus_rx_buffer[16] >> 1) | (sbus_rx_buffer[17] << 7)) &0x07ff;								//!< Channel 11
	sbus_channel[12] = ((sbus_rx_buffer[17] >> 4) | (sbus_rx_buffer[18] << 4)) &0x07ff;								//!< Channel 12
	sbus_channel[13] = ((sbus_rx_buffer[18] >> 7) | (sbus_rx_buffer[19] << 1) | (sbus_rx_buffer[20] << 9)) &0x07ff;	//!< Channel 13
	sbus_channel[14] = ((sbus_rx_buffer[20] >> 2) | (sbus_rx_buffer[21] << 6)) &0x07ff;								//!< Channel 14
	sbus_channel[15] = ((sbus_rx_buffer[21] >> 5) | (sbus_rx_buffer[22] << 3)) &0x07ff;								//!< Channel 15	
}

void timer_cb(void)
{
	if(count<100)
	{
		count++;
	}
	else
	{
		outOfControlFlag = 1;
	}
}

/*
	ch_num:channel number 1-12
*/
int sbus_get(int ch_num)
{
	if(ch_num > 0 && ch_num<=12)
	{
		return sbus_channel[ch_num-1];
	}
	
	return 0;
}

